﻿using System;
using System.Collections.Generic;
using System.Text;

namespace SSPComm.SSPTransport
{
    public class SSPTransportStuff
    {
        private static Byte STUFF_BYTE = 0x7F;

#region GetCRC
        static int[] CRC_Table ={ 
            0x0000,0x8005,0x800F,0x000A,0x801B,0x001E,0x0014,0x8011, 
            0x8033,0x0036,0x003C,0x8039,0x0028,0x802D,0x8027,0x0022, 
            0x8063,0x0066,0x006C,0x8069,0x0078,0x807D,0x8077,0x0072, 
            0x0050,0x8055,0x805F,0x005A,0x804B,0x004E,0x0044,0x8041, 
            0x80C3,0x00C6,0x00CC,0x80C9,0x00D8,0x80DD,0x80D7,0x00D2, 
            0x00F0,0x80F5,0x80FF,0x00FA,0x80EB,0x00EE,0x00E4,0x80E1, 
            0x00A0,0x80A5,0x80AF,0x00AA,0x80BB,0x00BE,0x00B4,0x80B1, 
            0x8093,0x0096,0x009C,0x8099,0x0088,0x808D,0x8087,0x0082, 
            0x8183,0x0186,0x018C,0x8189,0x0198,0x819D,0x8197,0x0192, 
            0x01B0,0x81B5,0x81BF,0x01BA,0x81AB,0x01AE,0x01A4,0x81A1, 
            0x01E0,0x81E5,0x81EF,0x01EA,0x81FB,0x01FE,0x01F4,0x81F1, 
            0x81D3,0x01D6,0x01DC,0x81D9,0x01C8,0x81CD,0x81C7,0x01C2, 
            0x0140,0x8145,0x814F,0x014A,0x815B,0x015E,0x0154,0x8151, 
            0x8173,0x0176,0x017C,0x8179,0x0168,0x816D,0x8167,0x0162, 
            0x8123,0x0126,0x012C,0x8129,0x0138,0x813D,0x8137,0x0132, 
            0x0110,0x8115,0x811F,0x011A,0x810B,0x010E,0x0104,0x8101, 
            0x8303,0x0306,0x030C,0x8309,0x0318,0x831D,0x8317,0x0312, 
            0x0330,0x8335,0x833F,0x033A,0x832B,0x032E,0x0324,0x8321, 
            0x0360,0x8365,0x836F,0x036A,0x837B,0x037E,0x0374,0x8371, 
            0x8353,0x0356,0x035C,0x8359,0x0348,0x834D,0x8347,0x0342, 
            0x03C0,0x83C5,0x83CF,0x03CA,0x83DB,0x03DE,0x03D4,0x83D1, 
            0x83F3,0x03F6,0x03FC,0x83F9,0x03E8,0x83ED,0x83E7,0x03E2, 
            0x83A3,0x03A6,0x03AC,0x83A9,0x03B8,0x83BD,0x83B7,0x03B2, 
            0x0390,0x8395,0x839F,0x039A,0x838B,0x038E,0x0384,0x8381, 
            0x0280,0x8285,0x828F,0x028A,0x829B,0x029E,0x0294,0x8291, 
            0x82B3,0x02B6,0x02BC,0x82B9,0x02A8,0x82AD,0x82A7,0x02A2, 
            0x82E3,0x02E6,0x02EC,0x82E9,0x02F8,0x82FD,0x82F7,0x02F2, 
            0x02D0,0x82D5,0x82DF,0x02DA,0x82CB,0x02CE,0x02C4,0x82C1, 
            0x8243,0x0246,0x024C,0x8249,0x0258,0x825D,0x8257,0x0252, 
            0x0270,0x8275,0x827F,0x027A,0x826B,0x026E,0x0264,0x8261, 
            0x0220,0x8225,0x822F,0x022A,0x823B,0x023E,0x0234,0x8231, 
            0x8213,0x0216,0x021C,0x8219,0x0208,0x820D,0x8207,0x0202};


        static public Byte[] GetCRC(Byte[] inData)
        {

            Byte crcL = 0xFF;
            Byte crcH = 0xFF;
            Int32 tableAddr;
            for (int i = 0; i < inData.Length; i++)
            {
                tableAddr = (inData[i] ^ crcH);
                crcH = (Byte)((CRC_Table[tableAddr] >> 8) ^ crcL);
                crcL = (Byte)((CRC_Table[tableAddr] & 0x00FF));
            }
            return new Byte[] { crcL, crcH };
        }

        static public Boolean IsCRCValid(Byte[] inData)
        {
            if (inData.Length < 3)
            {
                throw new Exception("No data to check crc");
            }
            Byte[] datawoCRC = new Byte[inData.Length - 2];
            Array.Copy(inData, 0, datawoCRC, 0, inData.Length - 2);
            Byte[] res = GetCRC(datawoCRC);
            return ((res[0] == inData[inData.Length - 2]) && 
                    (res[1] == inData[inData.Length - 1]));
        }

#endregion

        static public Byte[] InsertByteStuff(Byte[] inData)
        {
            List<Byte> result = new List<Byte>(10);
            //result.Add(inData[0]);

            for (int i = 0; i < inData.Length; i++)
            {
                if (inData[i] == STUFF_BYTE)
                {
                    result.Add(inData[i]);
                    result.Add(inData[i]);
                }
                else
                {
                    result.Add(inData[i]);
                }
            }
            return result.ToArray();
        }

        static public Byte[] DeleteByteStuff(Byte[] inData)
        {
            List<Byte> result = new List<Byte>(10);
            Boolean added = false;

            for (int i = 0; i < inData.Length; i++)
            {
                if ((i > 0) &&
                    (inData[i] == STUFF_BYTE) &&
                    (inData[i - 1] == STUFF_BYTE) &&
                    (!added))
                {
                    added = true;
                    continue;
                }
                else
                {
                    result.Add(inData[i]);
                    added = false;
                }
            }
            return result.ToArray();
        }
    }

    #if DEBUG
    namespace Testing
    {
        using NUnit.Framework;
        [TestFixture]
        public class SSPTransportStuffTest
        {
            [Test]
            public void GetCRC()
            {
                Byte[] data = new Byte[] { 0x90, 0x01, 0x11 };

                Assert.AreEqual(0x26, SSPTransportStuff.GetCRC(data)[0]);
                Assert.AreEqual(0x03, SSPTransportStuff.GetCRC(data)[1]);

                data = new Byte[] { 0x90, 0x0A, 0xF0, 0xE6, 0x01, 0x32, 0x00, 0x00, 0x00, 0x52, 0x55, 0x42 };

                Assert.AreEqual(0x7F, SSPTransportStuff.GetCRC(data)[0]);
                Assert.AreEqual(0xB9, SSPTransportStuff.GetCRC(data)[1]);

                data = new Byte[] { 0x01, 0x01, 0x00, 0x00, 0x00, 0xf0, 0x0b, 0xf9, 0xc9, 0x6d, 0x73, 0xdc, 0x18, 0xb7, 0x2f, 0xe5, 0x0a, 0x02, 0x00, 0x00, 0x00, 0xf0, 0xda, 0x01, 0x00, 0x00, 0x00, 0x00, 0x52, 0x55  };
                System.Diagnostics.Debug.Print(data.Length.ToString());
                Assert.AreEqual(0x13, SSPTransportStuff.GetCRC(data)[0]);
                Assert.AreEqual(0x4F, SSPTransportStuff.GetCRC(data)[1]);
            }

            [Test]
            public void InsertByteStuff()
            {
                Byte[] data = new Byte[] { 0x90, 0x01, 0x11 };
                Byte[] stuffed = SSPTransportStuff.InsertByteStuff(data);
                Assert.AreEqual(data, stuffed);
                stuffed = SSPTransportStuff.DeleteByteStuff(data);
                Assert.AreEqual(data, stuffed);

                data = new Byte[] { 0x90, 0x01, 0x7F, 0x7F, 0x23 };
                stuffed = SSPTransportStuff.InsertByteStuff(data);

                Assert.AreEqual(new Byte[] { 0x90, 0x01, 0x7F, 0x7F, 0x7F, 0x7F, 0x23 },
                                stuffed);
            }

            [Test]
            public void DeleteByteStuff()
            {
                Byte[] data = new Byte[] { 0x90, 0x01, 0x11 };
                Byte[] stuffed = SSPTransportStuff.DeleteByteStuff(data);
                Assert.AreEqual(data, stuffed);

                data = new Byte[] { 0x90, 0x01, 0x7F, 0x7F, 0x7F, 0x7F, 0x23 };
                stuffed = SSPTransportStuff.DeleteByteStuff(data);

                Assert.AreEqual(new Byte[] { 0x90, 0x01, 0x7F, 0x7F, 0x23 },
                                stuffed);
            }

            [Test]
            public void IsCRCValid()
            {
                Byte[] data = new Byte[] { 0x01, 0x03, 0x00, 0x00, 0x00, 0xf0, 0x73, 0xfa };
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));

                data = new Byte[] { 0x0a, 0x04, 0x00, 0x00, 0x00, 0xf0, 0xe6, 0x01, 0xc8, 0x00, 0x00, 0x00, 0x52, 0x55, 0x42, 0xe1, 0x5e };
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));

                data = new Byte[] { 0x10, 0x11, 0x7E, 0x79, 0xE2, 0x17, 0xD0, 0x4A, 0xD0, 0x2E, 0xD0, 0x89, 0x55, 0xFF, 0xC8, 0xA8, 0x9A, 0xDA, 0x9D, 0x1C, 0x06 };
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));


                data = new Byte[]{0x90, 0x11 , 0x7E , 
                    0xCB , 0x9B , 0xA8 , 0xA7 , 0xB7 , 0xAA , 0x67 , 0x0F , 0xC9 , 0x8A , 0xE7 , 0x37 , 0xD6 , 0x15 , 0xF4 , 0x88 , 0x27, 0x44};
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));

                data = new Byte[] {0x10,	0x21, 0x7E, 0xBE, 0xB6, 0xC1, 0x2E, 0x57, 0x2C, 0x99, 0x92, 0xA9, 0xE1, 0x91, 0xAE, 0x3D, 0x27, 0xC4, 0x48, 0x36, 0x5A, 0x1A, 0x9E, 0x0D, 0xA3, 0x7B, 0xFC, 0x5B, 0x71, 0x80, 0x7B, 0xAD, 0x60, 0x86, 0xFD, 0x9C, 0x36 	};
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));

                //0x00000000289f278a
                data = new Byte[] { 0x10, 0x11, 0x7E, 0x62, 0x03, 0xA3, 0xCC, 0xD9, 0x9A, 0x68, 0xA4, 0x84, 0x08, 0xBB, 0x09, 0x83, 0x2B, 0xEF, 0xFE, 0x73, 0xD7 };
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));

                data = new Byte[]{ 0x10, 0x11, 0x7E, 0xF9, 0x88, 0x3F, 0x54, 0xD0, 0x53, 0xA2, 0x9F, 0xCD, 0x09, 0xDE, 0x27, 0xB1, 0xA2, 0xB7, 0x89, 0xA9, 0x38};
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));

                data = new Byte[] { 0x03, 0x00, 0x00, 0x00, 0x00, 0x30, 0x05, 0x44, 0x2b, 0xd0, 0xa2, 0xad, 0xe2, 0x0e, 0x09, 0xed, 0x11, 0x01, 0x00, 0x00, 0x00, 0xf0, 0x53, 0x48, 0x30, 0x30, 0x30, 0x33, 0x36, 0x30, 0x34, 0x31 };
                Assert.AreEqual(false, SSPTransportStuff.IsCRCValid(data));


                data = new Byte[] { 0x04, 0x11, 0x00, 0x00, 0x00, 0x3e, 0x52, 0x55, 0x42, 0x2b, 0xd0, 0xa2, 0xad, 0xe2, 0x31, 0x73 };
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));
                data = new Byte[] { 0x04, 0x11, 0x00, 0x00, 0x00, 0x3e, 0x52, 0x55, 0x42, 0x2b, 0xd0, 0xa2, 0xad, 0xe2, 0x31, 0x73 };
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));
                data = new Byte[] { 0x05, 0x12, 0x00, 0x00, 0x00, 0xf0, 0x0a, 0x00, 0x00, 0x00, 0x48, 0xc0, 0x13, 0xc2, 0x35, 0xb2 };
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));
                data = new Byte[] { 0x01, 0x12, 0x00, 0x00, 0x00, 0x0a, 0x93, 0xa3, 0x73, 0x60, 0x2b, 0x66, 0x2c, 0xc5, 0x21, 0x9e };
                Assert.AreEqual(true, SSPTransportStuff.IsCRCValid(data));
            }
        }
    }

    #endif
}
